#include "ConvexPolygon2.h"
#include "Allocator.h"
#include <algorithm>
#include <numeric>
#include <boost/cast.hpp>


//////////////////////////////////////////////

class PolygonSimplex2 : public Simplex2
{
public:
	PolygonSimplex2(const ConvexPolygonalShape2& composed, std::size_t startIndex);

	//geometry
	virtual const Vector2 internalPoint() const	{	return composed_->internalPoint();	}	
	virtual const Vector2 startPoint() const	{	return composed_->getVertex(startIndex_);	}
	virtual const Vector2 endPoint() const		{	return composed_->getVertex(endIndex_);	}

	
	//for iterator
	bool isEmpty() const;
	bool next();

private:
	bool nextIndex(std::size_t& start, std::size_t& end) const;

private:
	const ConvexPolygonalShape2* composed_;
	std::size_t startIndex_;
	std::size_t endIndex_;
};


PolygonSimplex2::PolygonSimplex2(const ConvexPolygonalShape2& composed, std::size_t startIndex)
	: composed_(&composed), startIndex_(startIndex), endIndex_(startIndex + ((composed.countVertices() >= 2? 1 : 0)))
{
	assert(startIndex_ < composed_->countVertices());
	assert(endIndex_ < composed_->countVertices());
}

inline bool PolygonSimplex2::isEmpty() const
{
	return startIndex_ == endIndex_;
}

bool PolygonSimplex2::next()
{
	/** @todo encapsule index handling */
	const std::size_t nextStartIndex = startIndex_ + 1;
	const std::size_t nextEndIndex = nextStartIndex + 1;

	const std::size_t count = composed_->countVertices();
	if (nextEndIndex < count)
	{
		startIndex_ = nextStartIndex;
		endIndex_   = nextEndIndex;
		return true;
	}
	else if (nextEndIndex == count)
	{
		startIndex_ = nextStartIndex;
		endIndex_   = 0;
		return true;
	}
	else
	{
		startIndex_ = 0;
		endIndex_   = 0;
		return false;
	}
}


//////////////////////////////////////////////////////

/** @todo manage to remove initial test */
class PolygonalSimplexIterator2 : public ISimplexIterator2
{
public:
	/**@todo refactor */
	explicit PolygonalSimplexIterator2(const ConvexPolygonalShape2& composed)
		: composed_(&composed), current_(composed, 0)
	{
	}

	~PolygonalSimplexIterator2()
	{
	}

	virtual const PolygonSimplex2& current() const;
	virtual bool next();


private:
	const ConvexPolygonalShape2* composed_;
	PolygonSimplex2	             current_;
};

const PolygonSimplex2& PolygonalSimplexIterator2::current() const
{
	return current_;
}

bool PolygonalSimplexIterator2::next()
{
	return current_.next();
}

///////////////////////////////////////
ISimplexIterator2* ConvexPolygonalShape2::decompose(IStackAllocator& stack) const
{
	return stack.create<PolygonalSimplexIterator2>(*this);
}

///////////////////////////////////////

void ConvexPolygon2::addVertex(const Vector2& vertex)
{
	vertices_.push_back(vertex);
	needUpdate_ = true;
}

const Vector2 ConvexPolygon2::internalPoint() const
{
	updateInternalPoint();
	return internalPoint_;
}

void ConvexPolygon2::updateInternalPoint() const
{
	if (needUpdate_)
	{
		internalPoint_ = calcGravity();
		needUpdate_ = false;
	}
}

const Vector2 ConvexPolygon2::calcGravity() const
{
	Vector2 result = std::accumulate(vertices_.begin(), vertices_.end(), Vector2(0, 0));
	return (result /= vertices_.size());
}

/**
 *  @warning not efficient now because linear search ; O(N)
 *  @todo make efficient by contruct binary tree */
const Vector2 ConvexPolygon2::supportPoint(const Vector2& normal) const
{
	if (vertices_.size() < 2)
	{
		assert(!"a polygon must have 3 vertices at least!");
		return Vector2();
	}

	StackAllocator stack(256);
	VALIDATE_STACK(stack);
	ISimplexIterator2* it(decompose(stack));

	Vector2 result = it->current().startPoint();
	const Vector2 headNormal(it->current().edgeNormal());
	const Scalar headDet(det(headNormal, normal));
	Scalar prevDet = headDet;
	while (it->next())
	{
		const Vector2 curNormal(it->current().edgeNormal());
		Scalar curDet(det(curNormal, normal));
		if (prevDet >= 0 && curDet < 0)
		{
			result = it->current().startPoint();
			break;
		}
		prevDet = curDet;
	}
	stack.destroy(it);
	return result;
}

/////////////////////////////////////
// boundary
#include <boost/ref.hpp>



class LineLoopIterator : public ILinearEdgeIterator2
{
public:
	LineLoopIterator(const ConvexPolygonalShape2& polygon, IStackAllocator& stack);
	~LineLoopIterator();

	virtual const ILinearEdge2& current() const;
	virtual bool next();

private:
	ISimplexIterator2*	adaptee_;
	ILinearEdge2*		edge_;
	mutable IStackAllocator*	memberStack_;	//allocates member objects
};


LineLoopIterator::LineLoopIterator(const ConvexPolygonalShape2& polygon, IStackAllocator& stack)
: adaptee_(polygon.decompose(stack)), edge_(adaptee_->current().edge(stack)), memberStack_(&stack)
{
}

LineLoopIterator::~LineLoopIterator()
{
	memberStack_->destroy(edge_);
	memberStack_->destroy(adaptee_);
}

const ILinearEdge2& LineLoopIterator::current() const
{
	return *edge_;
}

bool LineLoopIterator::next()
{
	memberStack_->destroy(edge_);
	bool ret = adaptee_->next();
	edge_ = adaptee_->current().edge(*memberStack_);
	return ret;
}

ILinearEdgeIterator2* ConvexPolygonalShape2::boundary(IStackAllocator& stack) const
{
	return stack.create<LineLoopIterator>(*this, boost::ref(stack));
}
